/*
 * Leg.h
 *
 *  Created on: Feb 19, 2011
 *      Author: Anton
 */
#ifndef LEG_H_
#define LEG_H_
#define LINKS 3
#define LINK1LENGTH .15
#define LINK2LENGTH .381
#define LINK3LENGTH .381
#define LINK1MASS 8.6
#define LINK2MASS 2.6
#define LINK3MASS 1.2

#include "Link.h"
#include "Matrix.h"
#include <vector>
#include <iostream>
using std::vector;
using std::endl;
using std::cout;
using std::cin;


class Leg {
public:
	Leg() {
		links = new Link[LINKS];
		unit=0;
		x=0;
		y=0;
		z=0;
		desiredx=0;
		desiredy=0;
		desiredz=0;
	}

	virtual ~Leg() {
		delete[] links;
		links = NULL;
	}

	double getUnit() const { return unit; }
	double getX() const { return x; }
	double getY() const { return y; }
	double getZ() const { return z; }
	void setUnit(double unit) { this->unit = unit; }
	void setX(double x) { this->x = x; }
	void setY(double y) { this->y = y; }
	void setZ(double z) { this->z = z; }
	double getDesiredx() const { return desiredx; }
	double getDesiredy() const { return desiredy; }
	double getDesiredz() const { return desiredz; }
	void setDesiredx(double desiredx) { this->desiredx = desiredx; }
	void setDesiredy(double desiredy) { this->desiredy = desiredy; }
	void setDesiredz(double desiredz) { this->desiredz = desiredz; }

	Matrix forwardK() {
		Matrix FK = Matrix(4,4, 1.0,0.0,0.0,0.0, 0.0,1.0,0.0,0.0, 0.0,0.0,1.0,0.0, 0.0,0.0,0.0,1.0);
		for(int i=0; i<LINKS; i++){
			Matrix B = Leg::links[i].forwardK();
			FK = FK*B;
		}
		//cout << "LEG: " << FK << endl;
		return FK;
	}

	Link* links;

	Matrix inverseK() {
		double x = Leg::getDesiredx();
		double y = Leg::getDesiredy();
		double z = Leg::getDesiredz();
		double theta1 = Leg::links[0].getDH().getvalue(0,3);
		double theta2 = Leg::links[1].getDH().getvalue(0,3);
		double theta3 = Leg::links[2].getDH().getvalue(0,3);

		double theta1ik = atan2(z,x)*RAD2DEG-theta1-180;
		double r = sqrt(pow(x,2)+pow(z,2))-LINK1LENGTH;
		double l = pow(r,2) + pow(y,2);
		double d = (l-pow(LINK2LENGTH,2)-pow(LINK3LENGTH,3))/(2*LINK2LENGTH*LINK3LENGTH);

		double theta3ik=atan2(Leg::getUnit()*sqrt(1-pow(d,2)),d)*RAD2DEG;
		double theta2ik=atan2(y,r)*RAD2DEG-atan2(LINK3LENGTH*sin(theta3ik*DEG2RAD),LINK2LENGTH+LINK3LENGTH*cos(theta3ik*DEG2RAD))*RAD2DEG;

		theta2ik = -theta2ik-theta2;
		theta3ik = -theta3ik-theta3;

		Matrix m = Matrix(1,3,theta1ik-90,theta2ik,theta3ik);
		return m;
	}

private:
	double unit;
	double x;
	double y;
	double z;
	double desiredx;
	double desiredy;
	double desiredz;
	Matrix I;
};

#endif

